/*
Copyright 2016 fixstars

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

http ://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/

#include <stdlib.h>
#include <iostream>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/core/version.hpp>

#if CV_MAJOR_VERSION == 2

#include <opencv2/contrib/contrib.hpp>

#endif
#include <iostream>
#include<iomanip>
#include "fstream"
#include <libsgm.h>
#include <chrono>
#include "open_chisel/Chisel.h"

#include <dirent.h>
#include <open_chisel/ProjectionIntegrator.h>
#include <open_chisel/camera/PinholeCamera.h>
#include <open_chisel/camera/DepthImage.h>
#include <open_chisel/camera/ColorImage.h>
#include <open_chisel/pointcloud/PointCloud.h>
#include <open_chisel/truncation/QuadraticTruncator.h>
#include <open_chisel/weighting/ConstantWeighter.h>

#include <open_chisel/truncation/QuadraticTruncator.h>
#include <open_chisel/truncation/InverseTruncator.h>
#include "KittiReader.h"

typedef float DepthData;
typedef uint8_t ColorData;
using namespace std;

bool SaveMesh(chisel::ChiselPtr chiselMap, string file_name) {
    bool saveSuccess = chiselMap->SaveAllMeshesToPLY(file_name);
    return saveSuccess;
}

void IntegrateLastPointCloud(chisel::ChiselPtr chiselMap, chisel::ProjectionIntegrator projectionIntegrator,
                             const chisel::PointCloud& lastPointCloud,
                             float farPlaneDist, chisel::Transform lastPose) {
    cout << "Integrating point cloud";
    chiselMap->IntegratePointCloud(projectionIntegrator, lastPointCloud, lastPose, 0.1f, farPlaneDist);

    chiselMap->UpdateMeshes();


}
void IntegrateLastDepthImage(chisel::ChiselPtr chiselMap, chisel::ProjectionIntegrator projectionIntegrator,
                             std::shared_ptr<chisel::DepthImage<DepthData>> lastDepthImage,
                             chisel::PinholeCamera cameraModel, chisel::Transform lastPose) {

    cout << "CHISEL: Integrating depth scan";
    auto start = std::chrono::system_clock::now();
    chiselMap->IntegrateDepthScan<DepthData>(projectionIntegrator, lastDepthImage, lastPose,
                                             cameraModel);

    std::chrono::duration<double> elapsed = std::chrono::system_clock::now() - start;
    cout << "CHISEL: Done with scan" << elapsed.count() * 1000 << "ms";
//        PublishLatestChunkBoxes();
//        PublishDepthFrustum();

    start = std::chrono::system_clock::now();
    cout << "CHISEL: Updating meshes";
    chiselMap->UpdateMeshes();

    elapsed = std::chrono::system_clock::now() - start;
    cout << "CHISEL: Done with mesh" << elapsed.count() * 1000 << " ms";


}

int main(int argc, char *argv[]) {
    int s = 4;
    std::stringstream sequence;
    sequence << std::setfill('0') << std::setw(2) << s;
    string data_root = "/home/bobin/data/kitti/sequences/";
    std::string left_img_path_orig = data_root + sequence.str() + "/image_0";
    std::string right_img_path_orig = data_root + sequence.str() + "/image_1";
    std::string output_colored_disparitymap_orig =
            "/home/bobin/data/kitti/sequences/" + sequence.str() + "/disparity";
    string configFile = data_root + sequence.str() + "/KITTI04-12.yaml";
    string groundTruthFolder = data_root + sequence.str() + "/04.txt";
    system(("mkdir " + output_colored_disparitymap_orig).c_str());

    chisel::ChiselPtr chiselMap;
    int chunkSizeX, chunkSizeY, chunkSizeZ;
    chunkSizeX = chunkSizeY = chunkSizeZ = 32;
    float voxelResolution = 0.03;
    bool useColor = true;
    float truncationDistScale = 8.0;
    chiselMap.reset(new chisel::Chisel(Eigen::Vector3i(chunkSizeX, chunkSizeY, chunkSizeZ), voxelResolution, useColor));
    chisel::ProjectionIntegrator projectionIntegrator;
    chisel::TruncatorPtr truncator(new chisel::InverseTruncator(truncationDistScale));

    float carvingDist = 0.05;
    float weight = 1;
    projectionIntegrator.SetCentroids(chiselMap->GetChunkManager().GetCentroids());
    projectionIntegrator.SetTruncator(truncator);
    projectionIntegrator.SetWeighter(chisel::WeighterPtr(new chisel::ConstantWeighter(weight)));
    projectionIntegrator.SetCarvingDist(carvingDist);
    projectionIntegrator.SetCarvingEnabled(true);


    chisel::PinholeCamera pinholeCamera;
    cv::FileStorage fsSettings(configFile, cv::FileStorage::READ);
    if (!fsSettings.isOpened()) {
        cerr << "ERROR: Wrong path to settings" << endl;
        return -1;
    }

    float fx = fsSettings["Camera.fx"];
    float fy = fsSettings["Camera.fy"];
    float cx = fsSettings["Camera.cx"];
    float cy = fsSettings["Camera.cy"];
    float bf = fsSettings["Camera.bf"];
    int width = fsSettings["Camera.width"];
    int height = fsSettings["Camera.height"];
    chisel::Intrinsics intrinsics;
    intrinsics.SetFx(fx);
    intrinsics.SetFy(fy);
    intrinsics.SetCx(cx);
    intrinsics.SetCy(cy);
    pinholeCamera.SetIntrinsics(intrinsics);
    pinholeCamera.SetWidth(width);
    pinholeCamera.SetHeight(height);

    TrajectoryType gtTraj;   //ground truth trajectory


    LoadGroundTruthTraj(groundTruthFolder, gtTraj);
    struct dirent *ptr;
    DIR *dir;
    string PATH = left_img_path_orig;
    dir = opendir(PATH.c_str());
    int counter = 0;

    while ((ptr = readdir(dir)) != NULL) {
        if (ptr->d_name[0] == '.')
            continue;
        counter++;
    }

    for (int number = 0; number < counter; number++) {
        std::stringstream ss;
        ss << std::setfill('0') << std::setw(6) << number;

        std::string left_img_path = left_img_path_orig + "/" + ss.str() + ".png";
        std::string right_img_path = right_img_path_orig + "/" + ss.str() + ".png";
        std::string output_colored_disparitymap = output_colored_disparitymap_orig + "/" + ss.str() + ".png";

        cv::Mat left = cv::imread(left_img_path, 0);
        cv::Mat right = cv::imread(right_img_path, 0);

        int disp_size = 64;

        if (left.size() != right.size() || left.type() != right.type()) {
            std::cerr << "mismatch input image size" << std::endl;
            std::exit(EXIT_FAILURE);
        }

        int bits = 0;

        switch (left.type()) {
            case CV_8UC1:
                bits = 8;
                break;
            case CV_16UC1:
                bits = 16;
                break;
            default:
                std::cerr << "invalid input image color format" << left.type() << std::endl;
                std::exit(EXIT_FAILURE);
        }


        sgm::StereoSGM ssgm(left.cols, left.rows, disp_size, bits, 8, sgm::EXECUTE_INOUT_HOST2HOST);

        cv::Mat output(cv::Size(left.cols, left.rows), CV_8UC1);

        ssgm.execute(left.data, right.data, (void **) &output.data);
        output.convertTo(output, CV_32F);
        float *out_ptr = (float *)output.data;

        chisel::Transform pose(gtTraj[number].cast<float>());
#if USE_Depth
        shared_ptr<chisel::DepthImage<DepthData>> depthImage = make_shared<chisel::DepthImage<DepthData>>(width,
                                                                                                          height);
        int size = left.cols * left.rows;
        DepthData *depthImageData = depthImage->GetMutableData();
        for (int i = 0; i < size; i++) {
            depthImageData[i] = bf / out_ptr[i];
        }
        IntegrateLastDepthImage(chiselMap, projectionIntegrator, depthImage, pinholeCamera, pose);
        std::cout << "Done! Sequence " << s << " number " << number << " is saved." << std::endl;
#else
        chisel::PointCloud cloud;

        for (int j = 0; j < height; ++j) {
            for (int i = 0; i < width; ++i) {
                chisel::Vec3 pt;
                float d = bf / out_ptr[j * height + i];
                pt[0] = d * (i - cx) / fx;
                pt[1] = d * (j - cy) / fy;
                pt[2] = d ;
                cloud.AddPoint(pt);
            }
        }
        float farPlaneDist = 50.0;
        IntegrateLastPointCloud(chiselMap, projectionIntegrator, cloud, farPlaneDist, pose);
#endif


    }

    SaveMesh(chiselMap, "map.ply");



}

